
//  Solve3D.cpp
//  newAR
//
//  Created by Shangjin Zhai on 14-12-6.
//  Copyright (c) 2014ƒÍ Shangjin Zhai. All rights reserved.
//

#include "Solve3D.h"
#include <iomanip>

namespace Solve3D {
  
    Eigen::Matrix3d setRotationMatrix(double yaw,double pitch,double roll)
    {
        Eigen::Matrix3d rotationMatrix;
        const double cx = cos(pitch), sx = sin(pitch), cy = cos(roll), sy = sin(roll), cz = cos(yaw), sz = sin(yaw);
        const double cycz = cy * cz, sxsy = sx * sy, cysz = cy * sz;
        rotationMatrix(0, 0) = cycz - sxsy * sz;
        rotationMatrix(0, 1) = cysz + sxsy * cz;
        rotationMatrix(0, 2) = -cx * sy;
        
        rotationMatrix(1, 0) = -cx * sz;
        rotationMatrix(1, 1) = cx * cz;
        rotationMatrix(1, 2) = sx;
        
        rotationMatrix(2, 0) = sy * cz + sx * cysz;
        rotationMatrix(2, 1) = sy * sz - sx * cycz;
        rotationMatrix(2, 2) = cx * cy;
        Eigen::Matrix3d Rk;
        Rk.setZero();
        Rk(0,1) = -1;
        Rk(1,0) = -1;
        Rk(2,2) = -1;
        rotationMatrix = Rk*rotationMatrix;
        //    cout<<rotationMatrix<<endl;
        return rotationMatrix;
    }
    
    
    void get3DPoints(const cv::Matx33d& KMatrix,const cv::Matx33d &rotationMatrix, const cv::Size &imgSize, Mat &pts3D,Matx31d &t)
{
    pts3D = Mat(imgSize,CV_32FC3);
    Matx33d minusK, minusR;
    invert(KMatrix, minusK, CV_SVD);
//    cout<<KMatrix<<endl;
//    cout<<minusK<<endl;
    minusR = rotationMatrix.t();
//    cout<<rotationMatrix<<endl;
    Matx33d mRK = minusR*minusK;
//    cout<<minusR<<mRK<<endl;
    CV_Assert(pts3D.isContinuous());
    double *m = mRK.val;
    float *dat =(float*)pts3D.data;
#define REAL_HEIGHT 5.0
    for (int i=0; i<imgSize.height; ++i) {
        for (int j=0; j<imgSize.width; ++j) {
            float s = -REAL_HEIGHT/(m[6]*j+m[7]*i+m[8]);
            dat[0] = (m[0]*j+m[1]*i+m[2])*s;
            dat[1] = (m[3]*j+m[4]*i+m[5])*s;
            dat[2] = 0;
            dat+=3;
        }
    }
    Matx31d CC = Matx31d::zeros();
    CC(0) = 0;
    CC(1) = 0;
    CC(2) = REAL_HEIGHT;
    t = -rotationMatrix * CC;//t = -RC

}


    void get3DPoints(const Eigen::Matrix3d& KMatrix,const Eigen::Matrix3d &rotationMatrix,vector<KeyPoint> &pts2D,vector<Point3f> &pts3D,Eigen::Vector3d &TVector)
{
#define REAL_HEIGHT 5.0
    
    Eigen::Matrix3d minusK, minusR;
    minusK = KMatrix.inverse();
    minusR = rotationMatrix.inverse();
    
    for (int i=0; i<pts2D.size(); i++)
    {
        
        cv::Point3f point3D;
        Eigen::Vector3d imageCoordinate;
        Eigen::Vector3d realCoordinate;
        
        imageCoordinate(2, 0) = 1;
        double realHeight = REAL_HEIGHT, realScale = 1;
        
        imageCoordinate(0, 0) = pts2D[i].pt.x;
        imageCoordinate(1, 0) = pts2D[i].pt.y;
        
        Eigen::Vector3d right = minusR * minusK * imageCoordinate;
        realScale = -realHeight / right(2, 0);
        
        point3D.x = right(0, 0) * realScale ;
        point3D.y = right(1, 0) * realScale ;
        point3D.z = right(2, 0) * realScale + REAL_HEIGHT;
        
        //cout<<"TVector:"<<TVector<<endl;
        cout<<point3D<<endl;
        pts3D.push_back(point3D);
    }

    
    
}
    
    Eigen::Matrix3d HfromPose(const Eigen::Matrix3d&K,const Pose&C1,const Pose &C2,const Eigen::Vector3d&N,const double&D)
    {
        
        Eigen::Matrix3d R12 = C2.R*C1.R.transpose();
        Eigen::Vector3d t12 = C2.t-R12*C1.t;
        auto R1Nt =  (C1.R*N).transpose();
        auto s =R1Nt*C1.t;
        Eigen::Matrix3d H = K*(R12-t12*R1Nt*(1/(D-s(0))))*K.inverse();
        H *= (1.0/(H(2,2)));
        return H;
        
    }

void get3DPoints(Mat KMatrix,const Mat rotationMatrix,const Mat TVector,const vector<KeyPoint> &pts2D,vector<Point3f> &pts3D)
{
    double height = TVector.at<float>(2,0);

    pts3D.clear();
    cv::Point3d point3D;
    Mat imageCoordinate = Mat::Mat(3, 1, CV_32F);
    Mat realCoordinate;
    Mat minusK, minusR;
    invert(KMatrix, minusK, CV_SVD);
    invert(rotationMatrix, minusR, CV_SVD);
    
    for (int i = 0; i < pts2D.size(); i++)
    {
        imageCoordinate.at<float>(2, 0) = 1;
        float realHeight = height, realScale = 1;
        
        imageCoordinate.at<float>(0, 0) = pts2D[i].pt.x;
        imageCoordinate.at<float>(1, 0) = pts2D[i].pt.y;
        
        Mat right = minusR * minusK * imageCoordinate;
        realScale = -realHeight / right.at<float>(2, 0);
        
        point3D.x = right.at<float>(0, 0) * realScale;//+ offset.x;
        point3D.y = right.at<float>(1, 0) * realScale;//+ offset.y;
        point3D.z = right.at<float>(2, 0) * realScale + height;
        pts3D.push_back(point3D);
    }
    
}


Mat getNextInvertMatrix(Mat KMatrix,Mat R,Mat t)
{
    Mat converseMatrix;
    Mat P = Mat(3, 4, CV_32F);
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            P.at<float>(i, j) = R.at<float>(i, j);
        }
        P.at<float>(i, 3) = (float)t.at<float>(i);
    }
    return converseMatrix = KMatrix * P;
//    invert(converseMatrix, invertMatrix, CV_SVD);
    
	 
}


    
    
    void get3DPoints(const Eigen::Matrix3d &KMatrix, const Eigen::Matrix3d& rotationMatrix, vector<vector<Feature*>> &pts2Ds,Eigen::Vector3d &TVector,float scale,int levelNum)
    {
        Eigen::Matrix3d minusK, minusR;
        minusK = KMatrix.inverse();
        minusR = rotationMatrix.inverse();
        for (size_t i = 0; i < pts2Ds.size(); i++)
        {
            auto &pts2D = pts2Ds[i];
            
            float s = pow(scale, i);
            for (int i = 0; i<pts2D.size(); i++)
            {
                
                markerAR::MapPoint *point3D =  new markerAR::MapPoint();
                Eigen::Vector3d imageCoordinate;
                Eigen::Vector3d realCoordinate;
                
                imageCoordinate(2, 0) = 1;
                double realHeight = REAL_HEIGHT, realScale = 1;
                
                imageCoordinate(0, 0) = pts2D[i]->_pos.x()*s;
                imageCoordinate(1, 0) = pts2D[i]->_pos.y()*s;
                
                Eigen::Vector3d right = minusR * minusK * imageCoordinate;
                realScale = -realHeight / right(2, 0);
                
                point3D->pointXYZ.x() = right(0, 0) * realScale;
                point3D->pointXYZ.y() = right(1, 0) * realScale;
                point3D->pointXYZ.z() = right(2, 0) * realScale + REAL_HEIGHT;
                pts2D[i]->pointXYZ = point3D;
            }
        }
        
        Eigen::Vector3d CC;
        CC.setZero();
        CC(2) = REAL_HEIGHT;
        TVector = -rotationMatrix * CC;//t = -RC
        
        
    }
    
    
    
void get3DPoints(Matx33d KMatrix, Matx33d rotationMatrix, vector<vector<KeyPoint>> &pts2Ds, vector<vector<Point3f>> &pts3Ds, Matx31d &TVector,float scale,int levelNum)
{
    TVector = Mat_<float>(3, 1);
    Matx33d minusK, minusR;
    invert(KMatrix, minusK, CV_SVD);
    invert(rotationMatrix, minusR, CV_SVD);
    for (size_t i = 0; i < pts2Ds.size(); i++)
    {
        auto &pts2D = pts2Ds[i];
        auto &pts3D = pts3Ds[i];

        float s = pow(scale, i);
        for (int i = 0; i<pts2D.size(); i++)
        {
            
            cv::Point3f point3D;
            Matx31d imageCoordinate = Mat::Mat(3, 1, CV_32F);
            Mat realCoordinate;
            
            imageCoordinate(2, 0) = 1;
            double realHeight = REAL_HEIGHT, realScale = 1;
            
            imageCoordinate(0, 0) = pts2D[i].pt.x*s;
            imageCoordinate(1, 0) = pts2D[i].pt.y*s;
            
            Matx31d right = minusR * minusK * imageCoordinate;
            realScale = -realHeight / right(2, 0);
            
            point3D.x = right(0, 0) * realScale;
            point3D.y = right(1, 0) * realScale;
            point3D.z = right(2, 0) * realScale + REAL_HEIGHT;
            
            pts3D.push_back(point3D);
        }
    }
    
    Matx31d CC = Matx31d::zeros();
    CC(2) = REAL_HEIGHT;
    TVector = -rotationMatrix * CC;//t = -RC
    
    
}

    
//some TYPE may cause overflow
template<typename TYPE>
double computeSSD(TYPE* a,TYPE* b,int length)
  {
      double ssd = .0;
    for (int i=0; i<length; i++)
    {
        ssd += (a-b)*(a-b);
    }
      return ssd;
  }


bool getCameraRT(const Matx33d &KMatrix,const vector<Point3f> &P3D,const vector<Point2f> &P2D,Matx33d &R,Matx31d &t,bool isTracked)
{
    CV_Assert(P3D.size()==P2D.size());

    Mat dtmp = Mat::zeros(4, 1, CV_64F);
    Matx31d rvec,TVector;
    Scalar origin;
    double err1=0, err2=0;
    if (P3D.size()<10) {
        return false;
    }
    
    if(!isTracked)
    {
        solvePnPRansac(P3D, P2D, KMatrix, Mat(), rvec, t,false,100,3,20);
    }
    else
    {
//        TVector = t;
//        Matx31d rr;
//        Rodrigues(R,rr);
//        FromRotationMatrix(R, origin);
//        vector<Point2f> pts2d;
//        projectPoints(P3D, rr, t, KMatrix, Mat(), pts2d);
//        for (int i = 0; i<pts2d.size(); i++)
//        {
//            err1 += sqrtf(pow(pts2d[i].x - P2D[i].x, 2) + pow(pts2d[i].y - P2D[i].y, 2));
//        }
//        err1 /= pts2d.size();
//        Mat mask;
        Rodrigues(R, rvec);
        TVector = t;
        solvePnP(P3D, P2D, KMatrix, Mat(), rvec, TVector,true);
        t = TVector;
        
        Matx33d RR;
        Rodrigues(rvec, RR);
        Scalar optimised;
        FromRotationMatrix(RR, optimised);
        Scalar origin;
        FromRotationMatrix(R, origin);
        double w = 0.8;
        Scalar dampq;
        Slerp(w, optimised, origin, dampq);
        Matx33d dampR;
        ToRotationMatrix(dampq, dampR);
        Rodrigues(dampR, rvec);
        Matx31d  tt;
        Tlerp(w, t, TVector, tt);
        t = tt;
        
//
//        
//        Matx33d RR;
//        Rodrigues(rr, RR);
//        
//        Scalar optimised;
//        FromRotationMatrix(RR, optimised);
//        projectPoints(P3D, rr, TVector, KMatrix, Mat(), pts2d);
//        
//        for (int i = 0; i<pts2d.size(); i++)
//        {
//            err2 += sqrtf(pow(pts2d[i].x - P2D[i].x, 2) + pow(pts2d[i].y - P2D[i].y, 2));
//        }
//        err2 /= pts2d.size();
//        if(err2>3.0)
//            return false;
//        //		CV_Assert(err2 <= err1);
//        double threshold = err2 * 1.5;
//        double w = 1;
//        Scalar output;
//        Matx31d tt;
//        for (int i = 0; i < 10; i++)
//        {
//            w -= 0.1;
//            double err3 = 0;
//            Slerp(w, optimised, origin, output);
//            ToRotationMatrix(output, RR);
//            Rodrigues(RR, rr);
//            Tlerp(w, t, TVector, tt);
//            projectPoints(P3D, RR, tt, KMatrix, Mat(), pts2d);
//            for (int j = 0; j<pts2d.size(); j++)
//            {
//                err3 += sqrtf(pow(pts2d[j].x - P2D[j].x, 2) + pow(pts2d[j].y - P2D[j].y, 2));
//            }
//            err3 /= pts2d.size();
//            if (err3>threshold)
//            {
//                break;
//            }
//        }
//        rvec = rr;
//        t = tt;
        
    }

    Rodrigues(rvec, R);
#ifdef _DEBUG
    cout<<"R:"<<R<<endl<<"t"<<t<<endl;
#endif
    return true;
}

double computeReprojectedError(const vector<Point3f> &objectPoints,const vector<Point2f> &imagePoints,const Matx33d&KMatrix,const Matx33d&R,const Matx31d&t)
{
    Mat rvec ;
    Rodrigues(R, rvec);
    vector<Point2f> rp;
    projectPoints(objectPoints, rvec, t, KMatrix, Mat(), rp);
    double ee = 0;
    for (int i=0; i<rp.size(); ++i) {
        ee+=sqrtf(pow(rp[i].x - imagePoints[i].x, 2) + pow(rp[i].y - imagePoints[i].y, 2));
    }
    return ee/rp.size();
}

    
    Eigen::Matrix4d PosetoGL(const Eigen::Matrix3d &R,const Eigen::Vector3d &t)
    {
        Eigen::Matrix4d PP;
        for(int i = 0; i < 3; i++)
        {
            for(int j = 0; j < 3; j++)
            {
                PP(i, j) = R(i, j);
                if(i == 2||i==1)
                    PP(i, j) = -R(i, j);
            }
            PP(i, 3) = t(i);
            if(i == 2||i==1)
                PP(i, 3) = -PP(i, 3);
        }
        PP(3, 0) =PP(3, 1) =PP(3, 2) =0;
        PP(3, 3) =1;
        return PP;
    }
    
Eigen::Matrix4d getP(const Eigen::Matrix3d &R,const Eigen::Vector3d &t)
{
    Eigen::Matrix4d PP;
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            PP(i, j) = R(i, j);
            if(i == 2)
                PP(i, j) = -R(i, j);
        }
        PP(i, 3) = t(i);
        if(i == 2)
            PP(i, 3) = -PP(i, 3);
    }
    PP(3, 0) =PP(3, 1) =PP(3, 2) =0;
    PP(3, 3) =1;
    return PP;
}

void getHighAndLow(vector<Point3f> &points3DVector,double &lowX, double &lowY, double &highX, double &highY)
{
    lowX = 100, lowY = 100, highX = -100, highY = -100;
    for(int i = 0; i < points3DVector.size() ; i++)
    {
        if(points3DVector[i].x < lowX)
            lowX = points3DVector[i].x;
        if(points3DVector[i].y < lowY)
            lowY = points3DVector[i].y;
        if(points3DVector[i].x > highX)
            highX = points3DVector[i].x;
        if (points3DVector[i].y > highY) {
            highY = points3DVector[i].y;
        }
    }
}

void printPlane(Mat KMatrix,Mat R,Mat t,cv::Mat &drawPlane, double lowX, double lowY, double highX, double highY)
{
    Mat converseMatrix = getNextInvertMatrix(KMatrix, R, t);
    vector<Point2d> drawLine;
    double stepX, stepY;
    double meshCnt = 11;
    
    stepX = (highX - lowX)/meshCnt;
    stepY = (highY - lowY)/meshCnt;
    //double step = MIN(stepX, stepY);
    
    for(double i = lowX; i <= highX; i += stepX)
    {
        drawLine.clear();
        for(double j = lowY; j <= highY; j += stepY)
        {
            Mat tmp = Mat(4, 1, CV_32F);
            tmp.at<float>(3) = 1;
            tmp.at<float>(0) = i;
            tmp.at<float>(1) = j;
            tmp.at<float>(2) = 0;
            Mat result = converseMatrix * tmp;
            drawLine.push_back(Point2d(result.at<float>(0)/result.at<float>(2), result.at<float>(1)/result.at<float>(2)));
            //printf("%lf %lf\n",result.at<double>(0)/result.at<double>(2), result.at<double>(1)/result.at<double>(2));
        }
        for(int j = 0; j < drawLine.size() - 1; j++)
        {
            line(drawPlane, drawLine[j], drawLine[j+1], Scalar(255, 0, 0));
        }
    }
    
    for(double i = lowY; i <= highY; i += stepY)
    {
        drawLine.clear();
        for(double j = lowX; j <= highX; j += stepX)
        {
            Mat tmp = Mat(4, 1, CV_32F);
            tmp.at<float>(3) = 1;
            tmp.at<float>(0) = j;
            tmp.at<float>(1) = i;
            tmp.at<float>(2) = 0;
            Mat result = converseMatrix * tmp;
            drawLine.push_back(Point2d(result.at<float>(0)/result.at<float>(2), result.at<float>(1)/result.at<float>(2)));
        }
        for(int j = 0; j < drawLine.size() - 1; j++)
        {
            line(drawPlane, drawLine[j], drawLine[j+1], Scalar(255, 0, 0));
        }
    }
}





void Tlerp(const double &w1, const Matx31d &origin, const Matx31d &optimised, Matx31d &output)
{
    const double* a = origin.val;
    const double* b = optimised.val;
    double *c = output.val;
    for (size_t i = 0; i < 3; i++)
    {
        c[i] = (1 - w1)*a[i] + b[i]*w1;
    }
}

void Slerp(const double &w1, const Scalar &q1, const Scalar &q2, Scalar &output)
{
    output[0] = q1.dot(q2);
    if (output[0] > 0.0)
    {
        if (output[0] > 1.0)
            output[0] = 0.0;
        //else if(output[0] < -1.0)
        //	output[0] = PI;
        else
            output[0] = acos(output[0]);
        if (fabs(output[0]) < DBL_EPSILON)
        {
            output = q1;
            return;
        }
        output[1] = 1 / sin(output[0]);
        const double s1 = sin(w1 * output[0]) * output[1];
        const double s2 = sin((1 - w1) * output[0]) * output[1];
        output[0] = s1 * q1[0] + s2 * q2[0];
        output[1] = s1 * q1[1] + s2 * q2[1];
        output[2] = s1 * q1[2] + s2 * q2[2];
        output[3] = s1 * q1[3] + s2 * q2[3];
    }
    else
    {
        output[0] = -output[0];
        if (output[0] > 1.0)
            output[0] = 0.0;
        //else if(output[0] < -1.0)
        //	output[0] = PI;
        else
            output[0] = acos(output[0]);
        if (fabs(output[0]) < DBL_EPSILON)
        {
            output = q1;
            return;
        }
        output[1] = 1 / sin(output[0]);
        const double s1 = sin(w1 * output[0]) * output[1];
        const double s2 = sin((1 - w1) * output[0]) * output[1];
        output[0] = s1 * q1[0] - s2 * q2[0];
        output[1] = s1 * q1[1] - s2 * q2[1];
        output[2] = s1 * q1[2] - s2 * q2[2];
        output[3] = s1 * q1[3] - s2 * q2[3];
    }
}


void ToRotationMatrix(const Scalar q, Matx33d &R)
{
    const double q00 = q[0] * q[0], q01 = q[0] * q[1], q02 = q[0] * q[2], q03 = q[0] * q[3];
    const double q11 = q[1] * q[1], q12 = q[1] * q[2], q13 = q[1] * q[3];
    const double q22 = q[2] * q[2], q23 = q[2] * q[3];
    double *p = R.val;
    p[0] = q11 + q22;				p[1] = q01 + q23;				p[2] = q02 - q13;
    p[3] = q01 - q23;				p[4] = q00 + q22;				p[5] = q12 + q03;
    p[6] = q02 + q13;			p[7] = q12 - q03;		p[8] = q00 + q11;
    p[0] = 1 - p[0] - p[0];	p[1] = p[1] + p[1];		p[2] = p[2] + p[2];
    p[3] = p[3] + p[3];		p[4] = 1 - p[4] - p[4];	p[5] = p[5] + p[5];
    p[6] = p[6] + p[6];		p[7] = p[7] + p[7];		p[8] = 1 - p[8] - p[8];
}

void FromRotationMatrix(const Matx33d &R, Scalar &q)
{
    const double * p = R.val;
    q[3] = p[0] + p[4] + p[8];
    if (q[3] > p[0] && q[3] > p[4] && q[3] > p[8])
    {
        q[3] = sqrt(q[3] + 1) * 0.5;
        q[2] = 0.25 / q[3];
        q[0] = (p[5] - p[7]) * q[2];
        q[1] = (p[6] - p[2]) * q[2];
        q[2] = (p[1] - p[3]) * q[2];
    }
    else if (p[0] > p[4] && p[0] > p[8])
    {
        q[0] = sqrt(p[0] + p[0] - q[3] + 1) * 0.5;
        q[3] = 0.25 / q[0];
        q[1] = (p[1] + p[3]) * q[3];
        q[2] = (p[2] + p[6]) * q[3];
        q[3] = (p[5] - p[7]) * q[3];
    }
    else if (p[4] > p[8])
    {
        q[1] = sqrt(p[4] + p[4] - q[3] + 1) * 0.5;
        q[3] = 0.25 / q[1];
        q[0] = (p[1] + p[3]) * q[3];
        q[2] = (p[5] + p[7]) * q[3];
        q[3] = (p[6] - p[2]) * q[3];
    }
    else
    {
        q[2] = sqrt(p[8] + p[8] - q[3] + 1) * 0.5;
        q[3] = 0.25 / q[2];
        q[0] = (p[2] + p[6]) * q[3];
        q[1] = (p[5] + p[7]) * q[3];
        q[3] = (p[1] - p[3]) * q[3];
    }
}

}




